/************************************************************************************
*	Copyright Paxs Electronics 2012													*
*   All Rights Reserved																*
*   The Copyright symbol does not also indicate public availability or publication.	*
* 																					*
* 								PAXS ELECTRONICS									*
* 																					*
*						APP:   QuaXcopter Application								*
* 																					*
* - Compiler:           Code Composer Studio (Licensed)								*
* - Supported devices:  All Stellaris Family Cortex-M3.								*
* - AppNote:																		*
*																					*
* 	Created by: 		Juan Francisco Gomez Ruiz									*
*   Date Created: 		7 - Jan - 2012												*
*   Contac				pacoduz@gmail.com											*
* 																					*
* 	Description: 		QuaXcopter Application.										*
*   Device supported and tested: - LM3S5749 - LM3S3748 -							*
* 																				2012*
*************************************************************************************

*************************************************************************************
* 	Revision History:
*
*   Date          CP#           Author
*   MM-DD-YY      XXXXX:1       Initials   Description of change
*   -----------   -----------   --------   ------------------------------------
*    01-07-12      	2.0             JFGR	Initial version. Events Engine 
* 	 01-10-12		2.0.1			JFGR	Initial version. 9DOF DRIVER addition
* 	 01-22-12		2.0.2			JFGR	Initial version. DCM algorithm addition
* 	 02-01-12		2.0.3			JFGR	Initial version. PID Control Loop
* 	 02-20-12		2.0.4			JFGR	Initial version. Temperatue Sensor Driver
* 	 03-05-12		2.0.5			JFGR	Initial version. TI AHRS GUI Protocol Communication
* 	 03-14-12		2.0.6			JFGR	Initial version. Digital Filter Driver - IIR & FIR.
* 	 04-16-12		2.0.7			JFGR	Initial version. SPI communication with Expansion Board.
* 	 05-01-12		2.0.8			JFGR	Initial Version. Data structure "NonBlocking" for TI AHRS GUI interface
* 	 05-09-12		2.0.8			JFGR	Initial Version. Data Structure "NonBlocking" for any PC terminal 
* 
*************************************************************************************

* 	File/   ""QuaXcopter Application""
* 
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
*  OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
*  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
*  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,  
*  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
*  DEALINGS IN THE SOFTWARE.
* 
*/

	#include "lm3s5749.h"
	#include "inc/hw_ints.h"
	#include "inc/hw_memmap.h"
	#include "inc/hw_sysctl.h"
	#include "inc/hw_types.h"
	#include "driverlib/rom.h"
	#include "driverlib/gpio.h"
	#include "driverlib/uart.h"
	#include "driverlib/timer.h"
	#include "driverlib/interrupt.h"
	#include "driverlib/sysctl.h"
	#include "driverlib/systick.h"
	//#include "inc/hw_ssi.h"
	//#include "driverlib/ssi.h"
	#include "stdio.h"
	#include "stdlib.h"
	#include "driver_led.h"
	#include "Imu9DOFDriver.h"
	#include "DigitalFilters.h"
	#include "Dcm.h"
	#include "Compass.h"
	#include "ControladoresPID.h"
	#include "ADCDriver.h"
	#include "DS16XXDriver.h"
	#include "MaxSonarDriver.h"
	#include "ExpansionBoardComm.h"
	#include "USARTCommunications.h"
	#include "QXCCommunications.h"
	#include "DataIntegrity.h"
	#define   GLOBAL_Q	16
	#include "IQmath/IQmathLib.h"
	#include "EventsEngine.h"
	#include "SystemEvents.h"
	
	#define ON						1
	#define OFF						0
/******************************************************************************
*							GLOBAL MACRO DEFINITIONS
******************************************************************************/
	#define	TEST_BENCH				0
	#define FLY						1
	#define QUAXCOPTER_MODE			(TEST_BENCH)
	
	#define BLUETOOTH				(OFF)
	#define XBEE					(ON)
	#define UART0					0			// USB Port
	#define UART1					1			// WIFLY Module
	#define PWM_MOTORS				(OFF)		//Effect only available for TEST BENCH MODE
	#define MOTOR_THRUST_MAX		(80)		// Thrust max = 100
	
	// VOLTAGE DIVIER RELATION		//
	#define BATTERY_PERCENT			(OFF)
	#define LSB_TO_VOLT				(0.00293255)
	#define VOLT_DIV_RELATION		(0.3475936) // Vout = Vin * VOLT_DIV_RELATION
	
	// CONTROL INPUTS RESTRICTIONS	//
	#define RAD_TO_DEGREES			(57.29577)
	#define DEGREES_TO_RAD			(0.0174533125)
	#define TILT_STEP				(1 * DEGREES_TO_RAD)
	#define MAX_ROLL_TILT			(10 * DEGREES_TO_RAD)
	#define MIN_ROLL_TILT			(-10 * DEGREES_TO_RAD)
	#define MAX_PITCH_TILT			(10 * DEGREES_TO_RAD)
	#define MIN_PITCH_TILT			(-10 * DEGREES_TO_RAD)
	
		
	
//		PLAY STATION CONTROLLER BUTTONS MASKs		//
	#define ANALOG_COMMANDS			0
	#define DIGITAL_COMMANDS		1
	
	// MASKs for the Second Byte
	#define TRIANGLE_BUTTON			0x1000
	#define CIRCLE_BUTTON			0x2000
	#define X_BUTTON				0x4000
	#define SQUARE_BUTTON			0x8000
	
	// MASKs for the first byte
	#define UP_BUTTON				0x10
	#define DOWN_BUTTON				0x40
	#define LEFT_BUTTON				0x80
	#define RIGHT_BUTTON			0x20
	#define SELECT_BUTTON			0x01
	#define START_BUTTON			0x08
	#define LEFT_JOY_BUTTON			0x02
	#define RIGHT_JOY_BUTTON		0x04


/******************************************************************************
*   					GLOBAL VARIABLE DEFINITIONS
******************************************************************************/
	volatile unsigned char DCMFlag=0;				//DCMFlag indicate when to run DCM & PID code
	volatile unsigned char ADCTimeFlag=0;			//ADCFalg indicate when to read ADC FIFO
	volatile unsigned char TemperatureSensorFlag=0;	//TemperatureSensorFlag indicates when to read the Sensor
	volatile unsigned char ExpansionBoardFlag=0;	//Expansion Board Flag indicate when to Write & read Expansion Board
	volatile unsigned char DisplayTerminalFlag=0;	//Display Terminal Flag indicates when to Display all data into a PC terminal
	volatile unsigned char DisplayMessageEnable=0;	//Flag that determine if QuaXcopter can send or not Message to the Server
	volatile unsigned char FlyTerminalFlag=0;		//Fly Terminal Flag indicates when to Transmit all data into a wireless transceiver

	volatile signed short InputThrust;	//Thrust desire by the Controller
	volatile unsigned char HoldSpeedFlag=0;
	volatile unsigned short PSPreviousButtonsState,PSButtonsState;
	
	volatile unsigned char ControlCommandType;
	
	volatile unsigned long ADCValues[5];
	volatile unsigned long HeightCM,ProcessorTemperature;
	_iq BatteryStatus;
	
	volatile char data;
	

/******************************************************************************
*   					GLOBAL FUNCTIONS PROTOTYPES
******************************************************************************/

	//	vfnInitializationUART0 - Function that configurate the UART-0 port at desire Baud Rate & Interruption //
	//	**NOTE: UART0 is attach by Hardware to an UART-USB Transceiver 
	void vfnInitializationUART0 (unsigned long BaudRate, unsigned char IntEnableF );
	
	//	vfnInitializationUART1 - Function that configurate the UART-1 port at desire Baud Rate & Interruption //
	//	**NOTE: UART1 is attach by Hardware to an BLUETOOTH.	
	void vfnInitializationUART1 (unsigned long BaudRate, unsigned char IntEnableF );
	
	//	VfnLEDInitialization  - Function to configurate the I/O PORT attach by Hardware, to Drive the LED	//
	void VfnLEDInitialization (void);	
	
	//	VfnLEDToggle - Function to Toggle the LED 	"Simply" //
	void VfnLEDToggle  (void);
	
	// 	VfnTempreatureSensoTick - Function that turn ON the TemperatureSensorFlag	// 
	//  so the VfnTemperatureSensor Event can be called from the Events Engine
	void VfnTempreatureSensoTick(void);
	
	//	VfnTemperatureSensor - Function "EVENT" that run the State Machine to read the I2C Temperature chip	//
	void VfnTemperatureSensor (void);
	
	//  VfnCORTEXM3ProcessorTemperature -  Function that return the Processor Temperature.
	signed char VfnCORTEXM3ProcessorTemperature (unsigned short ADCTemp);
	
	//	VfnQuaXcopterBatteryStatus - Function that return the status of the battery "Percent" attach to the QuaXcopter control Board
	_iq VfnQuaXcopterBatteryStatus (unsigned short ADCBattery);
	
	// 	VfnExnpansionBoardTick - Function that turn ON the ExpansionBoardFlag	// 
	//  so the VfnExpansionBoardWriteAndRead Event can be called from the Events Engine
	void VfnExpansionBoardTick(void);
	
	//	VfnExpansionBoardWriteAndRead - Function "EVENT" that run the State Machine to Write & Read, the Expansion Board	//
	void VfnExpansionBoardWriteAndRead (void);
	
	
	//	VfnTimerA0Initialization - Function to configurate the TimerA 0 to a specific PERIOD 200 HZ
	//	**NOTE: this Frequency it is critical to RUN the DCM algorithm and PID controllers
	//			if the frequency is modified by USER, DCM.h & ControladoresPID.h must be UPDATED
	void VfnTimer0AInitialization(unsigned short Frequency);
	
	//	VfnTimerA1Initialization - Function to configurate the TimerA 1 to a specific PERIOD 100 HZ
	//	**NOTE: this Frequency control the ADC values read operation.
	void VfnTimer1AInitialization(unsigned short Frequency);
	
	// Move the Outputs From the IMU to the digital Filter inputs	//
	void VfnIMUParametersFilter(void);
	
	// Timer "Tick" to activate Display State MAchine //
	void VfnDisplayTerminalTick (void);
	
	void VfnPSControl (void);
	
	// Timer "tick" to activate Data Transmission //
	void VfnControlTick (void);
	// State Machine to write data to WIRELESS MODULE	//
	void VfnControlhandle (void);
	

/******************************************************************************
*   				EVENTS ENGINE SETUP AND CONFIGURATION 
* 							EVENTs and TIMERs		
******************************************************************************/	
	////////////////////////////////////////////////////
	//  		EVENTs & TIMERs CONFIGURATION		  //
	//					EVENTs ENGINE				  //
	////////////////////////////////////////////////////
	#define LED_TIMER				8000	
	#define DATOS_TIMER				2000	
	#define TEMP_TIMER				200000
	#define EXPANSION_BOARD_TIMER	2000
	#define UART_SERVICE_TIMER		500
	//	Struct of Functions pointers	//
	const sEvent taEvents[] =
	{
		vfnTimeBasedEventsEngine,
		VfnTemperatureSensor,
		VfnExpansionBoardWriteAndRead,
		VfnDisplayDataIntoTerminal,
		VfnControlhandle,
	};	
	//	Struct of Functions pointer and Time of execution //
	const sPeriodicTimers taPeriodicTimers[] =
	{
		{	VfnLEDToggle,				LED_TIMER				},
		{	VfnDisplayTerminalTick,		DATOS_TIMER				},
		{	VfnControlTick,				UART_SERVICE_TIMER		},
		{	VfnTempreatureSensoTick, 	TEMP_TIMER				},
		{	VfnExpansionBoardTick,		EXPANSION_BOARD_TIMER	}
	};

/******************************************************************************
*   								MAIN CODE 	
* 							 - QUAXCOPTER APLICATION -
******************************************************************************/		
	int main(void)
	{
		////////////////////////////////////////////////
		//		SELECT MAIN CLOCK to RUN at 50MHZ	  //
		////////////////////////////////////////////////
		SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
   		
   		////////////////////////////////////////////////////////////////
		//	INITIALIZATION OF ALL PERIPHERALS & SOFTWARE MODULES	  //
		////////////////////////////////////////////////////////////////
		VfnLEDInitialization();
   		vfnInitializationUART0(115200,0);				//UART0 to 115200 Buads and No Interruption
   		//vfnInitializationUART1(9600,0);
   		Vfn9DOFDriverInitialization();
   		VfnDCMInitialization();
   		VfnPWMModuleConfiguration();
   		VfnPIDGainsConfiguration();
   		VfnTimer0AInitialization(50);					//Timer Period set to 200Hz = 0.005 seg. for DCM algorithm & PID controller
   		VfnTimer1AInitialization(100);					//Timer period set to 100Hz = 0.01 seg. ADC values read.
   		VfnDS16XXSensorInitialization(ONE_SHOT_MODE);	//I2C Temperature Sensor Configurated to ONE SHOT MODE
   		VfnSSIModuleInitialization(SSI1_MODULE,50000,8);
   		VfnAttentionPin();
   		VfnIIRFilterSetup();
   		VfnDisplayDataInitialization();
   		VfnADCInitialization();
   		//VfnUSARTRxCommunicationInit();
   		
   		//////////////////////////////////////////////////////
		//	EVENT ENGINE - EVENTs & TIMERs INITIALIZATION	//
		//////////////////////////////////////////////////////
		vfnEventsEngineInit();			
		vfnEventEnable(BASE_TIME_EVENT);
   		vfnPeriodicTimerEnable(LED_TIMER_ID);
   		
   		vfnEventEnable(TEMPERATURE_EVENT);
   		vfnPeriodicTimerEnable(TEMP_TIMER_ID);
   		
   		//vfnEventEnable(EXPANSION_BOARD_EVENT);
   		//vfnPeriodicTimerEnable(EXPANSION_BOARD_TIMER_ID);
   		
   		vfnEventEnable(CONTROL_EVENT);
   		vfnPeriodicTimerEnable(CONTROL_TIMER_ID);
   		
   		#if QUAXCOPTER_MODE == TEST_BENCH
   			vfnEventEnable(DISPLAY_TERMINAL_EVENT);
   			vfnPeriodicTimerEnable(DISPLAY_TIMER_ID);
   		#endif
   		
		//////////////////////////////////////////////
		//	"SYSTICK" TIMER CONFIGURATION At 1MHZ	//
		//////////////////////////////////////////////
    	SysTickPeriodSet(SysCtlClockGet() / 100000);
    	SysTickEnable();
    	SysTickIntEnable();
    	IntMasterEnable();
    	
    	//////////////////////////////////////////////
		//		TURN ON MOTORS - TEST BENCH ONLY	//
		//////////////////////////////////////////////
    	#if PWM_MOTORS == ON && QUAXCOPTER_MODE == TEST_BENCH
    		volatile unsigned long dummy,i;
    		for(dummy=0;dummy<=5000000;dummy++)
    		{ VfnLEDToggle();}
    		for(dummy=0;dummy<=5000000;dummy++)
    		{ VfnLEDToggle();}
    		for(i=0;i<=40;i++)
			{
				VfnMainMotorsSpeed(i);
				for(dummy=0;dummy<=400000;dummy++);
			}
		#endif
		
		#if QUAXCOPTER_MODE == FLY	
			volatile unsigned long dummy,i;
			for(dummy=0;dummy<=5000000;dummy++)
    		{ VfnLEDToggle();}
    	#endif
    	//////////////////////////////////////////////
		//					MAIN LOOP				//
		//////////////////////////////////////////////
    	while(1)
    	{
//    		GPIO_PORTD_DATA_R |= 0x80;
//    		GPIO_PORTD_DATA_R &=~ 0x80;
			if(DCMFlag == 1)
			{
				DCMFlag = 0;
				VfnDCMUpdate();
				VfnStabilize();
			}
			Vfn9DOFDriver();
			if(NewData == 1)
			{
				NewData = 0;
				VfnIMUParametersFilter();
				VfnIIRFilter();
			}
			vfnEventsEngine();
			if(TemperatureSensorFlag == 1)
			{
				FAST_EVENT_SET(TEMPERATURE_EVENT);
			}
			if(ADCTimeFlag == 1)
			{
				ADCTimeFlag = 0;		
				ProcessorTemperature_t = (short)VfnCORTEXM3ProcessorTemperature(ADCValues[2]);
				HeightCM_t = ( unsigned short)VfnMaxSonarDistanceRead(ADCValues[0]);
				BatteryStatus_t = VfnQuaXcopterBatteryStatus(ADCValues[1]);
				ProcessorTemperature = ProcessorTemperature_t;
				HeightCM = HeightCM_t;
				BatteryStatus = BatteryStatus_t;
				ADCProcessorTrigger(ADC0_BASE, 1);
				ADCProcessorTrigger(ADC0_BASE, 2);
				ADCProcessorTrigger(ADC0_BASE, 3);
			}
			if(ExpansionBoardFlag == 1)
			{
				FAST_EVENT_SET(EXPANSION_BOARD_EVENT);
			}
			if(DisplayTerminalFlag == 1 && DisplayMessageEnable == 1)
			{
				FAST_EVENT_SET(DISPLAY_TERMINAL_EVENT);
			}
			if(FlyTerminalFlag == 1)
			{
				FAST_EVENT_SET(CONTROL_EVENT);
			}
			if(DisplayStatus.ActualState == STATE_FINISH)
			{
				DisplayTerminalFlag=0;
				DisplayStatus.ActualState = STATE_FILL;
			}
			
			if(NewDataControl == 1)
			{
				char ControlFlag;
				signed char SRoll,SPitch,SYaw,HoldFlag;
				short SHeight;
				NewDataControl=0;
				ControlFlag = VfnMessageManager(UART0, &SRoll,&SPitch,&SYaw,&SHeight,&HoldFlag);	
				if(ControlFlag == 1)
				{
					DisplayMessageEnable = 1;	
				}
				if(ControlFlag == 2)
				{
					DisplayMessageEnable = 1;
					if(SRoll == 1)
					{
						SetRollAngle = _IQ(MAX_ROLL_TILT);
					}
					if(SRoll == -1)
					{
						SetRollAngle = -_IQ(MIN_ROLL_TILT);
					}
					if(SRoll == 0)
					{
						SetRollAngle = _IQ(0.0);
					}
					if(SPitch == 1)
					{
						SetPitchAngle = _IQ(MAX_PITCH_TILT);
					}
					if(SPitch == -1)
					{
						SetPitchAngle = -_IQ(MIN_PITCH_TILT);
					}
					if(SPitch == 0)
					{
						SetPitchAngle = _IQ(0.0);
					}
					if(SYaw == 1)
					{
						SetYawAngle += _IQ(TILT_STEP);
					}
					if(SYaw == -1)
					{
						SetYawAngle -= _IQ(TILT_STEP);
					}
					if(HoldFlag != 1)
					{
						if(SHeight == 1)
						{
							QuadThrust +=100;
						}
						if(SHeight == -1)
						{
							QuadThrust -=100;
						}
					}
				}
				if(ControlFlag < 1 || ControlFlag > 2)
				{
					 DisplayMessageEnable = 0;
				}
			}
    	}
	}
	
		//////////////////////////////////////////////////////////////////////		
		//					INTERRUP HANDLER FUNCTIONS ZONE					//
		//////////////////////////////////////////////////////////////////////
	/*****************************************************************************
	* Function: IntSystickHandler
	* Description: Set the vfnTimeBasedEventsEngine Event
	* Caveats:  Interrup handler
 	*****************************************************************************/
	void IntSystickHandler (void)
	{
		FAST_EVENT_SET(BASE_TIME_EVENT);
	}
	// IntTimer0Handler - Function to set the DCMFlag that control DCM & PID algorithm
	/*****************************************************************************
	* Function: IntTimer0AHandler
	* Description: Set the DCMFlag that control DCM & PID algorithm, At speciffic Interval time
	* Caveats:  Any variation on the Frequency interrupt *MUST equal to Macro definitions in
	* 			DCM H file and PID control H file. Interrup handler.
 	*****************************************************************************/
	void IntTimer0AHandler (void)
	{
		TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
		DCMFlag = 1;
	}
	
	/*****************************************************************************
	* Function: IntTimer0BHandler
	* Description: Set the ADCTimeFlag that indicate when to read the ADC FIFO 
	* Caveats:  N/A
 	*****************************************************************************/
	void IntTimer1AHandler (void)
	{
		TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
		ADCTimeFlag = 1;
	}
	
	/*****************************************************************************
	* Function: 	IntSSI1ServiceHandler
	* Description: 	Interruption service routine to SPI1 Module
	* Caveats:  	It onl service to RX FIFO Buffer
 	*****************************************************************************/
	void IntSSI1ServiceHandler (void)
	{
		PackageDataCounter = VfnSPIReadRXBuffer();
		SSIIntClear(SSI1_BASE,SSI_RXFF );
	}
	
	/*****************************************************************************
	* Function: 	IntSequen1Handler
	* Description: 	Interruption service routine to READ ADC values.
	* Caveats:  	It read the Sequencer 1 FIFO buffer. and exits Int. service.
 	*****************************************************************************/
	void IntSequen1Handler (void)
	{
		volatile long i;
		ADCIntClear(ADC_BASE, 1);
		i=ADCSequenceDataGet(ADC0_BASE, 1, (unsigned long*)&ADCValues[2]);
		
	}
	
	/*****************************************************************************
	* Function: 	IntSequen1Handler
	* Description: 	Interruption service routine to READ ADC values.
	* Caveats:  	It read the Sequencer 1 FIFO buffer. and exits Int. service.
 	*****************************************************************************/
	void IntSequen2Handler (void)
	{
		volatile long i;
		ADCIntClear(ADC_BASE, 2);
		i=ADCSequenceDataGet(ADC0_BASE, 2, (unsigned long*)&ADCValues[1]);
		
	}
	
	/*****************************************************************************
	* Function: 	IntSequen1Handler
	* Description: 	Interruption service routine to READ ADC values.
	* Caveats:  	It read the Sequencer 1 FIFO buffer. and exits Int. service.
 	*****************************************************************************/
	void IntSequen3Handler (void)
	{
		volatile long i;
		ADCIntClear(ADC_BASE, 3);
		i=ADCSequenceDataGet(ADC0_BASE, 3, (unsigned long*)&ADCValues[0]);
		
	}
	
	/*****************************************************************************
	* Function: 	IntUART0__Handler
	* Description: 	Interruption service routine to READ UART0 data.
	* Caveats:  	It reads all data between these characters '<' & '>', then it will set NEWDATACONTROL variable.
 	*****************************************************************************/
	void IntUART0__Handler (void)
	{
		UARTIntClear(UART0_BASE,UART_INT_RX);
		data = (char)UARTCharGetNonBlocking(UART0_BASE);
		if(DataControlEngage == 1)
		{
			if( data == '>')
			{
				//*DataControlPtr = data;
				NewDataControl = 1;
				DataControlEngage = 0;
			}
			else
			{	
				*DataControlPtr = data;
				DataControlPtr++;
			}
		}
		if(data == '<' && DataControlEngage == 0 && NewDataControl == 0)
		{
			DataControlPtr = &DataControlBuffer[0];
			//*DataControlPtr = data;
			//DataControlPtr++;
			DataControlEngage = 1;
		}
			
	}

/*******************************************************************************************
 ******************************************************************************************/
 
			//////////////////////////////////////////////////
			//				LOCAL FUNCTIONS ZONE			//
			//////////////////////////////////////////////////

/*****************************************************************************
 * Function:	VfnTimer0AInitialization
 * Description:	Timer0 A initialization at specific Frequency and, enable overflow interrup
 * Parameters:	Frequency.
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/		
	void VfnTimer0AInitialization(unsigned short Frequency)
	{
		
		SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);	
		//
    	// Configure Timer0A as a 16-bit periodic timer.
    	//
    	TimerConfigure(TIMER0_BASE, TIMER_CFG_16_BIT_PAIR | TIMER_CFG_A_PERIODIC);
		TimerPrescaleSet(TIMER0_BASE,TIMER_A,11);
    	//
    	// Set the Timer0A load value to 1ms.
    	//
    	TimerLoadSet(TIMER0_BASE, TIMER_A, (SysCtlClockGet() / Frequency));
    	 //
    	// Configure the Timer0A interrupt for timer timeout.
    	//
    	TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);

    	//
    	// Enable the Timer0A interrupt on the processor (NVIC).
    	//
    	IntEnable(INT_TIMER0A);
    	//
   	 	// Enable Timer0A.
    	//
    	TimerEnable(TIMER0_BASE, TIMER_A);
	}

/*****************************************************************************
 * Function:	VfnTimer1AInitialization
 * Description:	Timer1 A initialization at specific Frequency and, enable overflow interrup
 * Parameters:	Frequency.
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/		
	void VfnTimer1AInitialization(unsigned short Frequency)
	{
		
		SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);	
		//
    	// Configure Timer1A as a 16-bit periodic timer.
    	//
    	TimerConfigure(TIMER1_BASE, TIMER_CFG_16_BIT_PAIR | TIMER_CFG_A_PERIODIC);
		TimerPrescaleSet(TIMER1_BASE,TIMER_A,11);
    	//
    	// Set the Timer1A load value to 1ms.
    	//
    	TimerLoadSet(TIMER1_BASE, TIMER_A, (SysCtlClockGet() / Frequency));
    	 //
    	// Configure the Timer1A interrupt for timer timeout.
    	//
    	TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);

    	//
    	// Enable the Timer1A interrupt on the processor (NVIC).
    	//
    	IntEnable(INT_TIMER1A);
    	//
   	 	// Enable Timer1A.
    	//
    	TimerEnable(TIMER1_BASE, TIMER_A);
	}
	
/*****************************************************************************
 * Function:	vfnInitializationUART0
 * Description:	 Function that configurate the UART-0 port at desire Baud Rate & Interruption
 * Parameters:	Baud Rate  & Interruption
 * Caveats:		UART0 is attach by Hardware to an UART-USB Transceiver
 * Return:		N/A
 *****************************************************************************/		 
	void vfnInitializationUART0 (unsigned long BaudRate,unsigned char IntEnableF )
	{
		SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

    	GPIOPinConfigure(GPIO_PA0_U0RX);
    	GPIOPinConfigure(GPIO_PA1_U0TX);

    	GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

   		UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), BaudRate,(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));

   		
   		UARTIntEnable(UART0_BASE,UART_INT_RX);
   		IntEnable(INT_UART0);
   		
   		UARTEnable(UART0_BASE);
   		UARTFIFODisable(UART0_BASE);
	}
	

/*****************************************************************************
 * Function:	vfnInitializationUART1
 * Description:	 Function that configurate the UART-1 port at desire Baud Rate & Interruption
 * Parameters:	Baud Rate  & Interruption
 * Caveats:		UART1 is attach by Hardware to a BLUETOOTH Transceiver.
 * Return:		N/A
 *****************************************************************************/			
	void vfnInitializationUART1 (unsigned long BaudRate,unsigned char IntEnableF )
	{
		SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
    	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);

    	GPIOPinConfigure(GPIO_PD2_U1RX);
    	GPIOPinConfigure(GPIO_PD3_U1TX);

    	GPIOPinTypeUART(GPIO_PORTD_BASE, GPIO_PIN_2 | GPIO_PIN_3);

   		UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), BaudRate,(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));	
	}

/*****************************************************************************
 * Function:	VfnLEDInitialization
 * Description:	Function that enable Port and the specific pin, where the on Board LED is attached.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/		
	void VfnLEDInitialization (void)
	{
		unsigned long ulLoop;
		SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOD;
   		ulLoop = SYSCTL_RCGC2_R;

    	GPIO_PORTD_DIR_R = 0x80;
    	GPIO_PORTD_DEN_R = 0x80;
	}

/*****************************************************************************
 * Function:	VfnLEDToggle
 * Description:	Function that toggle the on Board LED
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/		
	void VfnLEDToggle  (void)
	{
		GPIO_PORTD_DATA_R ^= 0x80;
	}

/*****************************************************************************
 * Function:	VfnTempreatureSensoTick
 * Description:	Function that turn ON the TemperatureSensorFlag and VfnTemperatureSensor Event
 * Parameters:	
 * Caveats:		Function Activated by a Software Timer, provided by the Events Engine.
 * Return:		N/A
 *****************************************************************************/	
	void VfnTempreatureSensoTick (void)
	{
		TemperatureSensorFlag=1;
	}
	
/*****************************************************************************
 * Function:	VfnTemperatureSensor
 * Description:	Function/"EVENT" that run the State Machine to read the I2C Temperature chip.
 * 				Once the Temperature is Readed And, DATA STRUCTURE = TERMINAL send Temperature.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnTemperatureSensor (void)
	{
		char listo;
		listo = VfnDS16XXSensorDriver(ONE_SHOT_MODE);
		if (listo == 1)
		{
			listo = 0;
			TemperatureSensorFlag = 0;
		}
	}

/*****************************************************************************
 * Function:	VfnCORTEXM3ProcessorTemperature
 * Description:	Function That return the Processor internal Temperature
 * Parameters:	ADCTemp - ADC read from the Processor Temperature. 
 * Caveats:		Uses IQ Math functions.
 * 				The temperature sensor serves two primary purposes: 
 * 				1) to notify the system that internal temperature is too high 
 * 				   or low for reliable operation.
 * 				2) to provide temperature measurements for calibration
 *				   of the Hibernate module RTC trim value.
 * Return:		Processor Temperature from -55 to 125 decimal value.
 *****************************************************************************/		
	signed char VfnCORTEXM3ProcessorTemperature (unsigned short ADCTemp)
	{
		_iq Temperature;
		Temperature = -(_IQmpy(_IQ(ADCTemp),_IQ(0.00293255)) - _IQ(2.7));
		Temperature = _IQmpy(Temperature,_IQ(75)) - _IQ(55);
		volatile signed char temp;
		temp = _IQint(Temperature);
		return temp;
		//return (signed char)_IQint(Temperature);
	}

/*****************************************************************************
 * Function:	VfnQuaXcopterBatteryStatus
 * Description:	Function That return the Battery Percent. from 0% to 100%
 * Parameters:	ADCBattery - ADC read from The battery Voltage. 
 * Caveats:		Custom function -> QuaXcopter control Board
 * Return:		Battery Status "Percent" - from 0% to 100%
 *****************************************************************************/
	_iq VfnQuaXcopterBatteryStatus (unsigned short ADCBattery)
	{
		volatile _iq BatteryST;
		
		BatteryST = _IQmpy(_IQ(ADCBattery),_IQ(0.00293255)); //Voltage Battery
		BatteryST = _IQdiv( BatteryST,_IQ(VOLT_DIV_RELATION));
		// EQUATION that gives the Battery Percent	//
		#if BATTERY_PERCENT == ON
		BatteryST = _IQdiv( (BatteryST - _IQ(5.4)),_IQ(0.03) );
		#endif
		
		return BatteryST;
	}
	
/*****************************************************************************
 * Function:	VfnIMUParametersFilter
 * Description:	Function that moves the IMU data to Digital Filters inputs.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnIMUParametersFilter(void)
	{
		FilterInputs[0]=_IQ(XAxisAcceleration);
		FilterInputs[1]=_IQ(YAxisAcceleration);
		FilterInputs[2]=_IQ(ZAxisAcceleration);
		FilterInputs[3]=_IQ(XAxisAngularRate);
		FilterInputs[4]=_IQ(YAxisAngularRate);
		FilterInputs[5]=_IQ(ZAxisAngularRate);
	}

/*****************************************************************************
 * Function:	VfnExpansionBoardTick
 * Description:	Function that turn ON the ExpansionBoardFlag,
 * 				so the VfnExpansionBoardWriteAndRead Event can be called from the Events Engine
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnExpansionBoardTick(void)
	{
		ExpansionBoardFlag=1;
	}
	
/*****************************************************************************
 * Function:	VfnExpansionBoardWriteAndRead
 * Description:	Function/"EVENT" that run the State Machine to Write & Read the ExpnasionBoard
 * 				Once the Write & Read operation is done, update SetAngle - Roll and Pitch and, Motor Thrust
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/		
	void VfnExpansionBoardWriteAndRead (void)
	{
		volatile unsigned char Info,Validation;
		Info = VfnExpansionBoardComm();
		if(Info == 1)						//if new data is available, validate data.
		{
			Validation = VfnSPIValidateData();
			if(Validation == 1)
			{
				VfnPSControl();
				ExpansionBoardFlag=0;
			}
			
		}
	}

/*****************************************************************************
 * Function:	VfnPSControl
 * Description:	Function that read the Play Station Controller Status and change
 * 				modify parameters.
 * 				Parameters:
 * 				- SetPitchAngle
 * 				- SetRollAngle
 * 				- SetYawAngle
 * 				- QuadThrust
 * 				
 * 				 commands
 * 				- InputThrust "this parameter modify by left joystick only"
 * 				- START BUTTON holds InputThrust
 * 				- UP BUTTON increases Thrust by 0.5%
 * 				- DOWN BUTTON decreases Thrust by 0.5%
 * 				- SQUARE BUTTON decreases Roll Angle by 0.1 degrees
 * 				- CIRCLE BUTTON increases Roll Angle by 0.1 degrees
 * 				- TRIANGLE BUTTON decreases Pitch by 0.1 degrees
 * 				- X BUTTON increases Pitch by 0.1 degrees
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnPSControl (void)
	{
		signed char AnalogCommand;
		PSButtonsState =  (ExpansionBoardData[0] | (ExpansionBoardData[1]<<8)); 
		
		InputThrust    =  (unsigned short)ExpansionBoardData[4];

		// Software limiter - 1 Action, one time pressed button. 
		if(PSButtonsState == PSPreviousButtonsState)
		{
			PSButtonsState = 0;	
		}
		else
		{
			PSPreviousButtonsState = PSButtonsState;
		}
		
		// if Select Button is pressed, for the first time change from analog to digital
		// and, select button is pressed again, change from digital to analog.
		if((PSPreviousButtonsState & SELECT_BUTTON) && (ControlCommandType == DIGITAL_COMMANDS))
		{
			ControlCommandType = ANALOG_COMMANDS;
			PSPreviousButtonsState &=~ SELECT_BUTTON;
		}
		if(PSPreviousButtonsState & SELECT_BUTTON)
		{
			ControlCommandType ^= DIGITAL_COMMANDS;
			PSPreviousButtonsState &=~ SELECT_BUTTON;
		}
		
		
		// Endreass and Decreass Thrust section 
		if( PSButtonsState & UP_BUTTON )
		{
			QuadThrust+=100;
			if(QuadThrust> (BASE_TIME+BASE_TIME))
			{
				QuadThrust = (BASE_TIME+BASE_TIME);
			}
		}
		if( PSButtonsState & DOWN_BUTTON )
		{
			QuadThrust-=100;
			if(QuadThrust < BASE_TIME)
			{
				QuadThrust = BASE_TIME;
			}	
		}
		
		if(ControlCommandType == DIGITAL_COMMANDS)
		{
			if( PSButtonsState & SQUARE_BUTTON )
			{
				SetRollAngle -= _IQ(TILT_STEP);
				if(SetRollAngle < _IQ(MIN_ROLL_TILT) )
				{
					SetRollAngle = _IQ(MIN_ROLL_TILT);
				}
			}
			if( PSButtonsState & CIRCLE_BUTTON )
			{
				SetRollAngle += _IQ(TILT_STEP);
				if(SetRollAngle > _IQ(MAX_ROLL_TILT))
				{
					SetRollAngle = _IQ(MAX_ROLL_TILT);
				}
			}
			if( PSButtonsState & TRIANGLE_BUTTON )
			{
				SetPitchAngle -= _IQ(TILT_STEP);
				if(SetPitchAngle < _IQ(MIN_PITCH_TILT))
				{
					SetPitchAngle = _IQ(MIN_PITCH_TILT);
				}
			}
			if( PSButtonsState & X_BUTTON )
			{
				SetPitchAngle += _IQ(TILT_STEP);
				if(SetPitchAngle > _IQ(MAX_PITCH_TILT))
				{
					SetPitchAngle = _IQ(MAX_PITCH_TILT);
				}
			}
		}
		if(ControlCommandType == ANALOG_COMMANDS)
		{
			AnalogCommand  = ExpansionBoardData[2];
			if(AnalogCommand >= 5)
			{ 
				AnalogCommand = 5;
			}
			if(AnalogCommand <= -5)
			{
				AnalogCommand = -5;
			}
			SetPitchAngle  =  _IQmpy(_IQ(AnalogCommand),_IQ(DEGREES_TO_RAD));
			AnalogCommand  = ExpansionBoardData[3];
			if(AnalogCommand >= 5)
			{ 
				AnalogCommand = 5;
			}
			if(AnalogCommand <= -5)
			{
				AnalogCommand = -5;
			}
			SetRollAngle   =  _IQmpy(_IQ(AnalogCommand),_IQ(DEGREES_TO_RAD));
		}
	}
	
	
/*****************************************************************************
 * Function:	VfnDisplayTerminalTick
 * Description:	Function that turn ON the DisplayTerminal Flag and activated DisplayDataIntoTerminal Event
 * Parameters:	
 * Caveats:		Function Activated by a Software Timer, provided by the Events Engine.
 * Return:		N/A
 *****************************************************************************/		
	void VfnDisplayTerminalTick (void)
	{
		DisplayTerminalFlag=1;
	}

/*****************************************************************************
 * Function:	VfnFlyTerminalTick
 * Description:	Function that turn ON the FlyTerminal Flag and activated FlyDataIntoTerminal Event
 * Parameters:	
 * Caveats:		Function Activated by a Software Timer, provided by the Events Engine.
 * Return:		N/A
 *****************************************************************************/
	void VfnControlTick(void)
	{
		FlyTerminalFlag=1;
	}
/*****************************************************************************
 * Function:	VfnTransmitDataIntoTerminal
 * Description:	Function that handle the Transmission State Machine.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnControlhandle (void)
	{
		if(VfnUSARTRxCommunication())
    	{
    		VfnUSARTRxCommunicationInit();
    	}
	}
